﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace SmartMathLibrary
{
    /// <summary>
    /// This class provides a QR decomposition for real matrices.
    /// </summary>
    [Serializable]
    public class QRDecomposition : AbstractDecomposition
    {
        /// <summary>
        /// The number of columns of the specified matrix.
        /// </summary>
        private int n;

        /// <summary>
        /// The number of rows of the specified matrix.
        /// </summary>
        private int m;

        /// <summary>
        /// The qr data of the decomposition. 
        /// </summary>
        private double[,] qr;

        /// <summary>
        /// The data of the diagonal matrix of the decomposition.
        /// </summary>
        private double[] diagonal;

        /// <summary>
        /// The source matrix for the decomposition.
        /// </summary>
        private Matrix sourceMatrix;

        /// <summary>
        /// Initializes a new instance of the <see cref="QRDecomposition"/> class.
        /// </summary>
        /// <param name="sourceMatrix">The source matrix for the decomposition.</param>
        public QRDecomposition(Matrix sourceMatrix)
        {
            this.sourceMatrix = sourceMatrix;
        }

        /// <summary>
        /// Gets or sets the source matrix for the decomposition.
        /// </summary>
        /// <value>The source matrix for the decomposition.</value>
        public Matrix SourceMatrix
        {
            get { return sourceMatrix; }
            set { sourceMatrix = value; }
        }

        /// <summary>
        /// Gets the Householder vectors. Lower trapezoidal matrix whose columns define the reflections.
        /// </summary>
        /// <value>Lower trapezoidal matrix whose columns define the reflections.</value>
        public Matrix H
        {
            get
            {
                Matrix result = new Matrix(this.m, this.n);
                double[,] tempuri = result.MatrixData;

                for (int i = 0; i < this.m; i++)
                {
                    for (int j = 0; j < this.n; j++)
                    {
                        if (i >= j)
                        {
                            tempuri[i, j] = this.qr[i, j];
                        }
                        else
                        {
                            tempuri[i, j] = 0.0;
                        }
                    }
                }

                return result;
            }
        }

        /// <summary>
        /// Gets the upper triangular factor.
        /// </summary>
        /// <value>The upper triangular factor.</value>
        public Matrix R
        {
            get
            {
                Matrix result = new Matrix(this.n, this.n);
                double[,] tempuri = result.MatrixData;

                for (int i = 0; i < this.n; i++)
                {
                    for (int j = 0; j < this.n; j++)
                    {
                        if (i < j)
                        {
                            tempuri[i, j] = this.qr[i, j];
                        }
                        else if (i == j)
                        {
                            tempuri[i, j] = this.diagonal[i];
                        }
                        else
                        {
                            tempuri[i, j] = 0.0;
                        }
                    }
                }

                return result;
            }
        }

        /// <summary>
        /// Gets the (economy-sized) orthogonal factor.
        /// </summary>
        /// <value>The (economy-sized) orthogonal factor.</value>
        public Matrix Q
        {
            get
            {
                Matrix result = new Matrix(this.m, this.n);
                double[,] tempuri = result.MatrixData;

                for (int k = this.n - 1; k >= 0; k--)
                {
                    for (int i = 0; i < this.m; i++)
                    {
                        tempuri[i, k] = 0.0;
                    }

                    tempuri[k, k] = 1.0;

                    for (int j = k; j < this.n; j++)
                    {
                        if (this.qr[k, k] != 0)
                        {
                            double s = 0.0;

                            for (int i = k; i < this.m; i++)
                            {
                                s += this.qr[i, k] * tempuri[i, j];
                            }

                            s = -s / this.qr[k, k];

                            for (int i = k; i < this.m; i++)
                            {
                                tempuri[i, j] += s * this.qr[i, k];
                            }
                        }
                    }
                }

                return result;
            }
        }

        /// <summary>
        /// Executes the QR decomposition.
        /// </summary>
        public override void ExecuteDecomposition()
        {
            this.qr = this.sourceMatrix.Copy().MatrixData;
            this.m = this.sourceMatrix.Rows;
            this.n = this.sourceMatrix.Columns;
            this.diagonal = new double[this.n];

            for (int k = 0; k < this.n; k++)
            {
                // Compute 2-norm of k-th column without under/overflow.
                double nrm = 0;

                for (int i = k; i < this.m; i++)
                {
                    nrm = Hypot(nrm, this.qr[i, k]);
                }

                if (nrm != 0.0)
                {
                    // Form k-th Householder vector.
                    if (this.qr[k, k] < 0)
                    {
                        nrm = -nrm;
                    }

                    for (int i = k; i < this.m; i++)
                    {
                        this.qr[i, k] /= nrm;
                    }

                    this.qr[k, k] += 1.0;

                    // Apply transformation to remaining columns.
                    for (int j = k + 1; j < this.n; j++)
                    {
                        double s = 0.0;

                        for (int i = k; i < this.m; i++)
                        {
                            s += this.qr[i, k] * this.qr[i, j];
                        }

                        s = -s / this.qr[k, k];

                        for (int i = k; i < this.m; i++)
                        {
                            this.qr[i, j] += s * this.qr[i, k];
                        }
                    }
                }

                this.diagonal[k] = -nrm;
            }
        }

        /// <summary>
        /// Euclidean distance function.
        /// </summary>
        /// <param name="a">The first value.</param>
        /// <param name="b">The second value.</param>
        /// <returns>The computed distance.</returns>
        private static double Hypot(double a, double b)
        {
            double r;

            if (Math.Abs(a) > Math.Abs(b))
            {
                r = b / a;
                r = Math.Abs(a) * Math.Sqrt(1 + r * r);
            }
            else if (b != 0)
            {
                r = a / b;
                r = Math.Abs(b) * Math.Sqrt(1 + r * r);
            }
            else
            {
                r = 0.0;
            }

            return r;
        }
    }
}